#include "stm32f10x.h"                  // Device header
#include "MyI2C.h"
#include "MyHMC5883L.h"
#include "Delay.h"
#include "OLED.h"
#include "math.h"
#include "GPS_LUXIAN.h"
#define alpha 0.2

/**
  * 函    数：HMC5883L写
  * 参    数：RegAddress地址信息
  * 返 回 值：无
  * 注意事项：无
  */
void HMC5883L_WriteReg(uint8_t RegAddress, uint8_t Data)
{
	MyI2C_Start();						
	MyI2C_SendByte(HMC5883L_WRITE_ADDRESS);	
	MyI2C_ReceiveAck();					
	MyI2C_SendByte(RegAddress);		
	MyI2C_ReceiveAck();					
	MyI2C_SendByte(Data);			
	MyI2C_ReceiveAck();				
	MyI2C_Stop();					
}

/**
  * 函    数：HMC5883L读
  * 参    数：RegAddress地址信息
  * 返 回 值：无
  * 注意事项：无
  */
uint8_t HMC5883L_ReadReg(uint8_t RegAddress)
{
	uint8_t Data;
	
	MyI2C_Start();						
	MyI2C_SendByte(HMC5883L_WRITE_ADDRESS);	
	MyI2C_ReceiveAck();					
	MyI2C_SendByte(RegAddress);			
	MyI2C_ReceiveAck();				
	
	MyI2C_Start();					
	MyI2C_SendByte(HMC5883L_READ_ADDRESS | 0x01);
	MyI2C_ReceiveAck();				
	Data = MyI2C_ReceiveByte();		
	MyI2C_SendAck(1);			
	MyI2C_Stop();	
	
	return Data;
}

/**
  * 函    数：HMC5883L初始化
  * 参    数：无
  * 返 回 值：无
  * 注意事项：无
  */
void HMC5883L_Init(void)
{
	MyI2C_Init();
	HMC5883L_WriteReg(HMC5883L_REG_CONFIG_A, 0X70);
	HMC5883L_WriteReg(HMC5883L_REG_CONFIG_B, 0X20);
	HMC5883L_WriteReg(HMC5883L_REG_MODE, 0X00);
}

/**
  * 函    数：HMC5883L输出数据
  * 参    数：各磁场强度
  * 返 回 值：无
  * 注意事项：无
  */
void HMC5883L_GetData(int16_t *GaX, int16_t *GaY, int16_t *GaZ)
{
	uint8_t DataH, DataL;								
	
	DataH = HMC5883L_ReadReg(HMC5883L_REG_OUT_X_M);		
	DataL = HMC5883L_ReadReg(HMC5883L_REG_OUT_X_L);		
	*GaX = (DataH << 8) | DataL;						
	
	DataH = HMC5883L_ReadReg(HMC5883L_REG_OUT_Z_M);		
	DataL = HMC5883L_ReadReg(HMC5883L_REG_OUT_Z_L);		
	*GaY = (DataH << 8) | DataL;						
	
	DataH = HMC5883L_ReadReg(HMC5883L_REG_OUT_Y_M);		
	DataL = HMC5883L_ReadReg(HMC5883L_REG_OUT_Y_L);		
	*GaZ = (DataH << 8) | DataL;						
}

void jiaozhun()
{
		uint16_t i=0 ;
	
		int16_t GaX,GaY,GaZ,GaXmax=0,GaXmin=0,GaYmax=0,GaYmin=0;
	
	
		while(i != 300)
		{
			HMC5883L_GetData(&GaX, &GaZ,&GaY);
		
			GaXmax = GaXmax < GaX? GaX:GaXmax;
		
			GaXmin = GaXmin > GaX? GaX:GaXmin;
		
			GaYmax = GaYmax < GaY? GaY:GaYmax;
		
			GaYmin = GaYmin > GaY? GaY:GaYmin;
				
			Delay_ms(200);
			
			i++;
		}
	
		float Xoffest = (GaXmax+GaXmin)/2;
		float Yoffest = (GaYmax+GaYmin)/2;
		float Kx = 2/(GaXmax-GaXmin);
		float Ky = 2/(GaXmax-GaXmin);
		OLED_ShowSignedNum(1,1,Xoffest,5);
		OLED_ShowSignedNum(2,1,Yoffest,5);
		OLED_ShowSignedNum(3,1,Kx,5);
		OLED_ShowSignedNum(4,1,Ky,5);
}

void HMC5883L_get_angle(int16_t* now_angle)
{
	int16_t  X , Y , Z;
	HMC5883L_GetData(&X,&Z,&Y);
	Y = (double)Y;//54
	X = (double)X;//150
	*now_angle = atan2((double)Y,(double)X)*(180 / 3.14159265) +360;
	if(*now_angle>360)
	{
		*now_angle-=360;
	}
}



